// --------------------------------------------------------------------------------------------------------------------
// <copyright file="ManipulationPipelineApiTests.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------
namespace Test.Robotics.Hardware.KukaLBR.KukaLBR4Tests
{
    using System;
    using System.IO;
    using System.Reflection;
    using System.Runtime.Serialization;
    using System.Threading;
    using Microsoft.Robotics.Hardware.KukaLBR;
    using Microsoft.Robotics.Manipulation.ControllerStateMachine;
    using Microsoft.Robotics.Numerics;
    using Microsoft.VisualStudio.TestTools.UnitTesting;
    using Test.Robotics.Numerics;

    using KR = Microsoft.Robotics.Hardware.KukaLBR.KukaLBR4PlusCsharpDefinitionsNativeMethods;

    /// <summary>
    /// Validates the manipulation pipeline API
    /// </summary>
    [TestClass]
    public class ManipulationPipelineApiTests
    {
        /// <summary>
        /// Starts the pipeline API and checks that messages make it through to the FRI
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void ManipulationApiTestKuka()
        {
            FriSimulator apiTestFRI = new FriSimulator();
            Assert.IsTrue(apiTestFRI.DoNothingCount == 0);
            Assert.IsTrue(apiTestFRI.CloseCount == 0);
            Assert.IsTrue(apiTestFRI.DoCartesianImpedanceControlCount == 0);
            Assert.IsTrue(apiTestFRI.DoJointImpedanceControlCount == 0);
            Assert.IsTrue(apiTestFRI.DoPositionControlCount == 0);
            Assert.IsTrue(apiTestFRI.GetRequestCount == 0);
            Assert.IsTrue(apiTestFRI.InitializeCount == 0);
            Assert.IsTrue(apiTestFRI.WaitOnConnectCount == 0);

            using (KukaPipelineAPI armApi = KukaPipelineAPI.Create(apiTestFRI))
            {
                Assert.IsTrue(this.WaitForPropertyGreaterThan(apiTestFRI, "WaitOnConnectCount", 0, 1000) > 0, "ManipulationApi did not connect");
                Assert.IsTrue(this.WaitForPropertyGreaterThan(apiTestFRI, "DoNothingCount", 0, 1000) > 0, "ManipulationApi did not run");

                WorkingRegisterMessageManaged working = new WorkingRegisterMessageManaged();
                working.CartesianPositionCommandRegister = HomogenousTransform.ToFrameArray(
                    HomogenousTransformTest.GetRandomHomogenousTransform());

                Assert.IsTrue(armApi.SetWorkingRegisters(working), "Set working registers failed");
                int count = this.WaitForPropertyGreaterThan(apiTestFRI, "DoNothingCount", 0, 1000);
                Assert.IsTrue(this.WaitForPropertyGreaterThan(apiTestFRI, "DoNothingCount", count, 1000) > 0, "ManipulationApi did not run");

                FeedbackMessageManaged feedback = armApi.GetFeedbackMessage();
                Assert.IsNotNull(feedback, "Failed to get arm feedback");
            }
        }

        /// <summary>
        /// Basic test to start stop the state machine
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void ManipulationApiStateMachineTestKuka()
        {
            FriSimulator apiTestFRI = new FriSimulator();

            using (KukaPipelineAPI armApi = KukaPipelineAPI.Create(apiTestFRI))
            {
                unsafe
                {
                    using (ArmControlKuka arm = new ArmControlKuka(apiTestFRI))
                    {
                        KukaLBR4CartesianPositionController controller = new KukaLBR4CartesianPositionController(
                            new KukaLBR4WorkingRegisterManager(),
                            new Pose(),
                            1,
                            1,
                            1);

                        ControllerStateMachineDescription stateMachine = controller.BuildStateMachineDescription();

                        ControllerStateMachineDescription* stateMachinePtr = &stateMachine;
                        Assert.IsTrue(armApi.SetStateMachineMessage(stateMachinePtr));
                        Assert.IsTrue(armApi.StopStateMachine());
                    }
                }
            }
        }

        /// <summary>
        /// check that the default constructor works
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void TestFeedbackDefaultConstructor()
        {
            FeedbackMessageManaged feedback = new FeedbackMessageManaged();

            Assert.AreEqual(feedback.ControllerStrategyCommandRegister, 0.0, MathConstants.ErrorEpsilon, "ControllerStrategyCommandRegister set failed {0}", feedback.ControllerStrategyCommandRegister);
            Assert.AreEqual(feedback.Scalar1Register, 0.0, MathConstants.ErrorEpsilon, "Scalar1Register set failed");
            Assert.AreEqual(feedback.Scalar2Register, 0.0, MathConstants.ErrorEpsilon, "Scalar2Register set failed");
            Assert.AreEqual(feedback.Scalar3Register, 0.0, MathConstants.ErrorEpsilon, "Scalar3Register set failed");
            Assert.AreEqual(feedback.Scalar4Register, 0.0, MathConstants.ErrorEpsilon, "Scalar4Register set failed");
            Assert.AreEqual(feedback.Scalar5Register, 0.0, MathConstants.ErrorEpsilon, "Scalar5Register set failed");
            Assert.AreEqual(feedback.Scalar6Register, 0.0, MathConstants.ErrorEpsilon, "Scalar6Register set failed");
            Assert.AreEqual(feedback.Scalar7Register, 0.0, MathConstants.ErrorEpsilon, "Scalar7Register set failed");
            Assert.AreEqual(feedback.Scalar8Register, 0.0, MathConstants.ErrorEpsilon, "Scalar8Register set failed");
            Assert.AreEqual(feedback.TimestampInMsRegister, 0.0, MathConstants.ErrorEpsilon, "TimestampInMsRegister set failed");
            Assert.AreEqual(feedback.FriStateRegister, 0.0, MathConstants.ErrorEpsilon, "FriStateRegister set failed");
            Assert.AreEqual(feedback.UdpQualityRegister, 0.0, MathConstants.ErrorEpsilon, "UdpQualityRegister set failed");
            Assert.AreEqual(feedback.CycleTimeInMsRegister, 0.0, MathConstants.ErrorEpsilon, "CycleTimeInMsRegister set failed");
            Assert.AreEqual(feedback.SafetyLimitInPercentRegister, 0.0, MathConstants.ErrorEpsilon, "SafetyLimitInPercentRegister set failed");
            Assert.AreEqual(feedback.DrivePowerBitFieldRegister, 0.0, MathConstants.ErrorEpsilon, "DrivePowerBitFieldRegister set failed");
            Assert.AreEqual(feedback.ControllerStrategyRegister, 0.0, MathConstants.ErrorEpsilon, "ControllerStrategyRegister set failed");
            Assert.AreEqual(feedback.CurrentControllerStateNumber, 0.0, MathConstants.ErrorEpsilon, "CurrentControllerStateNumber set failed");

            Assert.AreEqual((uint)feedback.JointPositionCommandRegister.Length, Microsoft.Robotics.Hardware.KukaLBR.KukaLBR4PlusCsharpDefinitionsNativeMethods.NUM_JOINTS, "JointPositionCommandRegisterinitialized failed {0}", feedback.JointPositionCommandRegister.Length);
            Assert.AreEqual((uint)feedback.CartesianPositionCommandRegister.Length, Microsoft.Robotics.Hardware.KukaLBR.KukaLBR4PlusCsharpDefinitionsNativeMethods.CARTESIAN_FRAME_LENGTH, "CartesianPositionCommandRegisterinitialized failed");
            Assert.AreEqual((uint)feedback.JointTorqueCommandRegister.Length, Microsoft.Robotics.Hardware.KukaLBR.KukaLBR4PlusCsharpDefinitionsNativeMethods.NUM_JOINTS, "JointTorqueCommandRegisterinitialized failed");
            Assert.AreEqual((uint)feedback.TCPForceTorqueCommandRegister.Length, Microsoft.Robotics.Hardware.KukaLBR.KukaLBR4PlusCsharpDefinitionsNativeMethods.CARTESIAN_DIM, "TCPForceTorqueCommandRegisterinitialized failed");
            Assert.AreEqual((uint)feedback.JointStiffnessCommandRegister.Length, Microsoft.Robotics.Hardware.KukaLBR.KukaLBR4PlusCsharpDefinitionsNativeMethods.NUM_JOINTS, "JointStiffnessCommandRegisterinitialized failed");
            Assert.AreEqual((uint)feedback.JointDampingCommandRegister.Length, Microsoft.Robotics.Hardware.KukaLBR.KukaLBR4PlusCsharpDefinitionsNativeMethods.NUM_JOINTS, "JointDampingCommandRegisterinitialized failed");
            Assert.AreEqual((uint)feedback.CartesianStiffnessCommandRegister.Length, Microsoft.Robotics.Hardware.KukaLBR.KukaLBR4PlusCsharpDefinitionsNativeMethods.CARTESIAN_DIM, "CartesianStiffnessCommandRegisterinitialized failed");
            Assert.AreEqual((uint)feedback.CartesianDampingCommandRegister.Length, Microsoft.Robotics.Hardware.KukaLBR.KukaLBR4PlusCsharpDefinitionsNativeMethods.CARTESIAN_DIM, "CartesianDampingCommandRegisterinitialized failed");
            Assert.AreEqual((uint)feedback.CartVec1Register.Length, Microsoft.Robotics.Hardware.KukaLBR.KukaLBR4PlusCsharpDefinitionsNativeMethods.CARTESIAN_DIM, "CartVec1Registerinitialized failed");
            Assert.AreEqual((uint)feedback.CartVec2Register.Length, Microsoft.Robotics.Hardware.KukaLBR.KukaLBR4PlusCsharpDefinitionsNativeMethods.CARTESIAN_DIM, "CartVec2Registerinitialized failed");
            Assert.AreEqual((uint)feedback.CartVec3Register.Length, Microsoft.Robotics.Hardware.KukaLBR.KukaLBR4PlusCsharpDefinitionsNativeMethods.CARTESIAN_DIM, "CartVec3Registerinitialized failed");
            Assert.AreEqual((uint)feedback.JointVec1Register.Length, Microsoft.Robotics.Hardware.KukaLBR.KukaLBR4PlusCsharpDefinitionsNativeMethods.NUM_JOINTS, "JointVec1Registerinitialized failed");
            Assert.AreEqual((uint)feedback.JointVec2Register.Length, Microsoft.Robotics.Hardware.KukaLBR.KukaLBR4PlusCsharpDefinitionsNativeMethods.NUM_JOINTS, "JointVec2Registerinitialized failed");
            Assert.AreEqual((uint)feedback.JointVec3Register.Length, Microsoft.Robotics.Hardware.KukaLBR.KukaLBR4PlusCsharpDefinitionsNativeMethods.NUM_JOINTS, "JointVec3Registerinitialized failed");
            Assert.AreEqual((uint)feedback.Frame1Register.Length, Microsoft.Robotics.Hardware.KukaLBR.KukaLBR4PlusCsharpDefinitionsNativeMethods.CARTESIAN_FRAME_LENGTH, "Frame1Registerinitialized failed");
            Assert.AreEqual((uint)feedback.Frame2Register.Length, Microsoft.Robotics.Hardware.KukaLBR.KukaLBR4PlusCsharpDefinitionsNativeMethods.CARTESIAN_FRAME_LENGTH, "Frame2Registerinitialized failed");
            Assert.AreEqual((uint)feedback.Frame3Register.Length, Microsoft.Robotics.Hardware.KukaLBR.KukaLBR4PlusCsharpDefinitionsNativeMethods.CARTESIAN_FRAME_LENGTH, "Frame3Registerinitialized failed");
            Assert.AreEqual((uint)feedback.JointTemperatureRegister.Length, Microsoft.Robotics.Hardware.KukaLBR.KukaLBR4PlusCsharpDefinitionsNativeMethods.NUM_JOINTS, "JointTemperatureRegisterinitialized failed");
            Assert.AreEqual((uint)feedback.MeasuredJointPositionRegister.Length, Microsoft.Robotics.Hardware.KukaLBR.KukaLBR4PlusCsharpDefinitionsNativeMethods.NUM_JOINTS, "MeasuredJointPositionRegisterinitialized failed");
            Assert.AreEqual((uint)feedback.MeasuredJointVelocityRegister.Length, Microsoft.Robotics.Hardware.KukaLBR.KukaLBR4PlusCsharpDefinitionsNativeMethods.NUM_JOINTS, "EstimatedJointVelocityRegisterinitialized failed");
            Assert.AreEqual((uint)feedback.ReportedCommandedJointPositionRegister.Length, Microsoft.Robotics.Hardware.KukaLBR.KukaLBR4PlusCsharpDefinitionsNativeMethods.NUM_JOINTS, "ReportedCommandedJointPositionRegisterinitialized failed");
            Assert.AreEqual((uint)feedback.ReportedCommandedJointOffsetRegister.Length, Microsoft.Robotics.Hardware.KukaLBR.KukaLBR4PlusCsharpDefinitionsNativeMethods.NUM_JOINTS, "ReportedCommandedJointOffsetRegisterinitialized failed");
            Assert.AreEqual((uint)feedback.MeasuredCartesianPositionRegister.Length, Microsoft.Robotics.Hardware.KukaLBR.KukaLBR4PlusCsharpDefinitionsNativeMethods.CARTESIAN_FRAME_LENGTH, "MeasuredCartesianPositionRegisterinitialized failed");
            Assert.AreEqual((uint)feedback.ReportedCommandedCartesianPositionRegister.Length, Microsoft.Robotics.Hardware.KukaLBR.KukaLBR4PlusCsharpDefinitionsNativeMethods.CARTESIAN_FRAME_LENGTH, "ReportedCommandedCartesianPositionRegisterinitialized failed");
            Assert.AreEqual((uint)feedback.ReportedCommandedCartesianOffsetRegister.Length, Microsoft.Robotics.Hardware.KukaLBR.KukaLBR4PlusCsharpDefinitionsNativeMethods.CARTESIAN_FRAME_LENGTH, "ReportedCommandedCartesianOffsetRegisterinitialized failed");
            Assert.AreEqual((uint)feedback.MeasuredJointTorqueRegister.Length, Microsoft.Robotics.Hardware.KukaLBR.KukaLBR4PlusCsharpDefinitionsNativeMethods.NUM_JOINTS, "MeasuredJointTorqueRegisterinitialized failed");
            Assert.AreEqual((uint)feedback.MeasuredExternalJointTorqueRegister.Length, Microsoft.Robotics.Hardware.KukaLBR.KukaLBR4PlusCsharpDefinitionsNativeMethods.NUM_JOINTS, "MeasuredExternalJointTorqueRegisterinitialized failed");
            Assert.AreEqual((uint)feedback.EstimatedTCPForceTorqueRegister.Length, Microsoft.Robotics.Hardware.KukaLBR.KukaLBR4PlusCsharpDefinitionsNativeMethods.CARTESIAN_DIM, "EstimatedTCPForceTorqueRegisterinitialized failed");
            Assert.AreEqual((uint)feedback.JacobianRegister.Length, Microsoft.Robotics.Hardware.KukaLBR.KukaLBR4PlusCsharpDefinitionsNativeMethods.CARTESIAN_DIM * Microsoft.Robotics.Hardware.KukaLBR.KukaLBR4PlusCsharpDefinitionsNativeMethods.NUM_JOINTS, "JacobianRegisterinitialized failed");
            Assert.AreEqual((uint)feedback.MassMatrixRegister.Length, Microsoft.Robotics.Hardware.KukaLBR.KukaLBR4PlusCsharpDefinitionsNativeMethods.CARTESIAN_DIM * Microsoft.Robotics.Hardware.KukaLBR.KukaLBR4PlusCsharpDefinitionsNativeMethods.NUM_JOINTS, "MassMatrixRegisterinitialized failed");
            Assert.AreEqual((uint)feedback.GravityCompensationRegister.Length, Microsoft.Robotics.Hardware.KukaLBR.KukaLBR4PlusCsharpDefinitionsNativeMethods.NUM_JOINTS, "GravityCompensationRegisterinitialized failed");
            Assert.AreEqual((uint)feedback.LastIssuedJointPositionCommandRegister.Length, Microsoft.Robotics.Hardware.KukaLBR.KukaLBR4PlusCsharpDefinitionsNativeMethods.NUM_JOINTS, "LastIssuedJointCommandRegister failed");
        }

        /// <summary>
        /// initialize the API with a null pointer
        /// should not expect any exception here
        /// Test is currently turned off as it causes the controller thread to exit
        /// uncleanly, which leaves the test execution process in a bad state, causing
        /// a subsequent run of the test to crash the test execution process.  
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        [Ignore]
        public void KukaApiInitializeWithNullTest()
        {
            // No check here, just want to see if this goes through to cover the code
            FriSimulator apiTestFRI = null;
            KukaPipelineAPI armApi = KukaPipelineAPI.Create(apiTestFRI);
            armApi.Dispose();
        }

        /// <summary>
        /// Check for a property info on the KukaPipelineAPI 
        /// </summary>
        /// <param name="propertyName">Property name</param>
        /// <returns>PropertyInfo for the property</returns>
        private PropertyInfo GetProperty(string propertyName)
        {
            Type stateMachineType = typeof(FriSimulator);
            PropertyInfo propertyInfo = stateMachineType.GetProperty(propertyName);
            Assert.IsNotNull(propertyInfo, "Canot get property on pipelineAPI for " + propertyName);
            return propertyInfo;
        }

        /// <summary>
        /// Waits for property value to exceed a threshold
        /// </summary>
        /// <param name="armApi">The FRI simulator.</param>
        /// <param name="propName">Property name to check</param>
        /// <param name="threshold">Minimum threshold</param>
        /// <param name="maxWait">Maximum wait time</param>
        /// <returns>Final value.</returns>
        private int WaitForPropertyGreaterThan(FriSimulator armApi, string propName, int threshold, int maxWait)
        {
            int wait = 0;

            int count = 0;
            PropertyInfo propInfo = this.GetProperty(propName);
            do
            {
                count = (int)propInfo.GetValue(armApi, null);
                if (count > threshold)
                {
                    break;
                }

                Thread.Sleep(1);
                ++wait;
            } 
            while (wait < maxWait);

            return count;
        }
    }
}
